﻿using ROSBridgeLib;
using ROSBridgeLib.sensor_msgs;
using ROSBridgeLib.std_msgs;
using ROSBridgeLib.geometry_msgs;
using ROSBridgeLib.nav_msgs;
using SimpleJSON;
using UnityEngine;
using System.IO;
using System.Collections;


public class MapSuscribler : ROSBridgeSubscriber
{
    private static RosConnector connector = null;


    public new static string GetMessageTopic()
    {
        return "map";
    }

    public new static string GetMessageType()
    {
        return "nav_msgs/OccupancyGrid";
    }

    public new static ROSBridgeMsg ParseMessage(JSONNode msg)
    {
          
        return new OccupancyGrid(msg);

    }

    public new static void CallBack(ROSBridgeMsg msg)
    {
        
        if (connector == null)
        {

            GameObject gb=GameObject.Find("RosConnector");


            if (gb==null)
            {
                Debug.LogError("Can't find RosConnector GameObject, There must be a RosConnector in senese");
             
                return;
            }

            connector =gb.GetComponentInChildren<RosConnector>();



            if (connector==null)
            {
                Debug.LogError("Can't find RosConnector Compoment , There must be a RosConnector Compoment on RosConnector GameObject");

                return;
            }

        }
      
        OccupancyGrid map = (OccupancyGrid)msg;

        connector.OnGetMap(map);
       

    }


   

}
